#include <cstring>

#include "PacketBuffer.h"

static inline size_t circ_space(size_t head, size_t tail, size_t size)
{
	return (tail - head - 1) & (size - 1);
}

PacketBuffer::PacketBuffer(size_t size) :
	_size(size), _head(0), _tail(0), _packets(0)
{
	_data = new uint8_t[size];

	pthread_mutex_init(&_mutex, 0);
	pthread_mutex_init(&_packetsAvailableMutex, 0);
	pthread_cond_init(&_packetsAvailable, 0);
}

PacketBuffer::~PacketBuffer()
{
	pthread_mutex_destroy(&_mutex);
	pthread_mutex_destroy(&_packetsAvailableMutex);
	pthread_cond_destroy(&_packetsAvailable);

	delete[] _data;
}

int PacketBuffer::push(const Packet &pkt)
{
	ssize_t space = getSpace(sizeof(pkt._extra)
					+ pkt._extra.ipLen + pkt._extra.tcpLen);
	if (space < 0)
		return -1;

	space = putBytes(space, (uint8_t *)&pkt._extra, sizeof(pkt._extra));
	space = putBytes(space, pkt._ipData, pkt._extra.ipLen);
	putBytes(space, pkt._tcpData, pkt._extra.tcpLen);

	pthread_mutex_lock(&_packetsAvailableMutex);

	++_packets;
	if (_packets == 1)
		pthread_cond_signal(&_packetsAvailable);

	pthread_mutex_unlock(&_packetsAvailableMutex);

	return 0;
}

int PacketBuffer::pop(Packet &pkt)
{
	size_t avail;

	pthread_mutex_lock(&_packetsAvailableMutex);

	if (!_packets)
		pthread_cond_wait(&_packetsAvailable, &_packetsAvailableMutex);

	avail = _packets;
	if (_packets)
		--_packets;

	pthread_mutex_unlock(&_packetsAvailableMutex);

	if (!avail)
		return -1;

	size_t pos = getData(&avail);
	pos = getBytes(pos, (uint8_t *)&pkt._extra, sizeof(pkt._extra));
	pos = getBytes(pos, pkt._ipData, pkt._extra.ipLen);
	pos = getBytes(pos, pkt._tcpData, pkt._extra.tcpLen);
	popBytes(pos);

	return 0;
}

void PacketBuffer::lock(void)
{
	pthread_mutex_lock(&_mutex);
}

void PacketBuffer::unlock(void)
{
	pthread_mutex_unlock(&_mutex);
}

void PacketBuffer::popBytes(size_t pos)
{
	pos += sizeof(size_t) - 1;
	pos &= ~(sizeof(size_t) - 1);

	lock();

	_tail = pos;

	unlock();
}

size_t PacketBuffer::getBytes(size_t pos, uint8_t *data, size_t len)
{
	size_t data_to_end = _size - pos;

	if (data_to_end >= len) {
		memcpy(data, _data + pos, len);
		return (pos + len) & (_size - 1);
	}

	memcpy(data, _data + pos, data_to_end);
	memcpy(data + data_to_end, _data, len - data_to_end);
	return (pos + len) & (_size - 1);
}

size_t PacketBuffer::getData(size_t *avail)
{
	size_t pos;

	lock();

	pos = _tail;

	unlock();

	return getBytes(pos, (uint8_t *)avail, sizeof(*avail));
}

size_t PacketBuffer::putBytes(size_t pos, const uint8_t *data, size_t len)
{
	size_t space_to_end = _size - pos;

	if (space_to_end >= len) {
		memcpy(_data + pos, data, len);
		return (pos + len) & (_size - 1);
	}

	memcpy(_data + pos, data, space_to_end);
	memcpy(_data, data + space_to_end, len - space_to_end);
	return (pos + len) & (_size - 1);
}

ssize_t PacketBuffer::getSpace(size_t length)
{
	size_t aligned;

	aligned = (length + sizeof(size_t) + sizeof(size_t) - 1);
	aligned &= ~(sizeof(size_t) - 1);

	lock();

	if (circ_space(_head, _tail, _size) < aligned) {
		unlock();
		return -1;
	}

	size_t space = _head;
	_head += aligned;
	_head &= _size - 1;

	unlock();

	return putBytes(space, (uint8_t *)&length, sizeof(length));
}
